#ifndef BALANCECTRL_TASK_H
#define BALANCECTRL_TASK_H

#include "srpq_controller/FSM_States/ControlFSMData.h"
#include <common/Dynamics/Quadruped.h>
#include "common/cppTypes.h"

template <typename T>
class BalanceCtrlData {
public:
    Vec3<T> pBody_des;
    Vec3<T> vBody_des;
    Vec3<T> aBody_des;
    Vec3<T> pBody_RPY_des;
    Vec3<T> vBody_Ori_des;

    Vec3<T> pFoot_des[4];
    Vec3<T> vFoot_des[4];
    Vec3<T> aFoot_des[4];
    Vec3<T> Fr_des[4];

    Vec4<T> contact_state;
};

template <typename T>
class PIDCtrlData {
public:
    Vec3<T> qDes[4];
    Vec3<T> qdDes[4];
    Mat3<T> kpJoint[4]; 
    Mat3<T> kdJoint[4];  
};

template <typename T>
class BCoutputData {
public:    
    Vec3<T> qDes;
    Vec3<T> qdDes;
    Mat3<T> kpJoint; 
    Mat3<T> kdJoint;   
};

template <typename T>
class BalanceCtrlTask {
public:
    BalanceCtrlTask(){}
    BCoutputData<T> rs;
    void run(ControlFSMData<T>& data, BalanceCtrlData<T>* ctrl_data, int leg){
        task(data, ctrl_data, &rs, leg);
    }
    virtual void  task(ControlFSMData<T>& data, BalanceCtrlData<T>* ctrl_data, BCoutputData<T>* BCout, int leg) = 0;
};

#endif
